/*
 * @Author: Sian Li lisian_magic@163.com
 * @Date: 2024-08-23 14:45:54
 * @LastEditors: Sian Li lisian_magic@163.com
 * @LastEditTime: 2024-08-24 15:49:13
 * @FilePath: /hudar_verge/src/hudar_core/src/radar_mapping.cpp
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置
 * 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
#include "hudar_msg/msg/raw_locations.hpp"
#include <opencv2/opencv.hpp>
#include <rclcpp/rclcpp.hpp>

#include "rclcpp/rclcpp.hpp"
using namespace cv;
using namespace std;
using std::placeholders::_1;
class RadarMapping : public rclcpp::Node {
  public:
    RadarMapping() : Node("radar_mapping") {
        cv::namedWindow("radar_mapping", cv::WINDOW_FREERATIO);
        cv::resizeWindow("radar_mapping", 300, 560);
        map = cv::imread("properties/map.png");
        if (map.empty()) {
            RCLCPP_INFO(this->get_logger(), "Map Can Not Be Found! Exiting!");
            exit(-1);
        }
        loc_subscriber =
            this->create_subscription<hudar_msg::msg::RawLocations>(
                "/radar/raw_locations", 10,
                std::bind(&RadarMapping::map_callback, this, _1));
    }

  private:
    void map_callback(const hudar_msg::msg::RawLocations &msg) const {
        // RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg.data.c_str());
        Mat mapCur = map.clone();
        for (int i = 0; i < 12; ++i) {
            if (msg.x_robot[i] != 0.0 && msg.y_robot[i] != 0.0) {
                int xxx = int(msg.x_robot[i] * 100);
                int yyy = int(msg.y_robot[i] * 100);
                xxx = 2800 - xxx;
                yyy = 1500 - yyy;
                // RCLCPP_INFO(this->get_logger(), "%d, %d", xxx, yyy);
                circle(mapCur, Point(yyy, xxx), 60, cv::Scalar(69, 179, 84),
                       10);
                putText(mapCur, to_string(i), Point(yyy - 35, xxx + 35),
                        FONT_HERSHEY_SIMPLEX, 3, Scalar(87, 139, 46), 10, 8,
                        false);
            }
        }
        cv::imshow("radar_mapping", mapCur);
        if (cv::waitKey(10) == 27)
            exit(0);
    }
    Mat map;
    rclcpp::Subscription<hudar_msg::msg::RawLocations>::SharedPtr
        loc_subscriber;
};

int main(int argc, char *argv[]) {
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<RadarMapping>());
    rclcpp::shutdown();
    return 0;
}